//
// Created by xiang on 2022/3/15.
//
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/highgui.hpp>

#include "ch6/lidar_2d_utils.h"
#include "ch6/mapping_2d.h"
#include "common/io_utils.h"

DEFINE_string(file_path, "/home/zl/Public/kaist_data/urban_data/", "数据文件路径");
DEFINE_string(txt_path, "/home/zl/Public/kaist_data/urban_data/sensor_data/data_stamp.csv", "数据文件路径");
DEFINE_bool(with_loop_closing, true, "是否使用回环检测");

/// 测试2D lidar SLAM

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);
    
    sad::KaistCsvIO io(FLAGS_file_path, FLAGS_txt_path);
    sad::Mapping2D mapping;

    std::system("rm -rf ./data/ch6/*");

    if (mapping.Init(FLAGS_with_loop_closing) == false) {
        return -1;
    }

    io.SetScan2DProcessFunc([&](const Scan2d& scan) {
        Scan2dPtr current_scan = std::make_shared<Scan2d>(scan);

        return mapping.ProcessScan(current_scan);
    }).Go();


    cv::imwrite("./data/ch6/global_map.png", mapping.ShowGlobalMap(2000));
    return 0;
}